#include "vehicle_linear_lv.h"

namespace auto_ros
{
namespace control
{
VehicleLinearLv::VehicleLinearLv(
	double mass, double cf, double cr, double lf, double lr, double ix, double iz,
	double h, double k_phi, double d_phi, double max_lateral_acc,
	double max_tire_f_angle) : ContinuousLinearSys(6, 1, 2, 1), mass_(mass),
							   cf_(cf), cr_(cr), lf_(lf), lr_(lr), ix_(ix), iz_(iz),
							   h_(h), k_phi_(k_phi), d_phi_(d_phi),
							   max_lateral_acc_(max_lateral_acc),
							   max_tire_f_angle_(max_tire_f_angle),
							   base_state_matrix_(6, 6)

{

	speed_threshold_ = 1.0;
	theta1_ = cf_ + cr_;
	theta2_ = lf_ * cf_ - lr_ * cr_;
	theta3_ = lf_ * lf_ * cf_ + lr_ * lr_ * cr_;
	theta4_ = 1.0 / mass_ + h_ * h_ / ix_;
	theta5_ = -1.0 * k_phi_ * h_ / ix_ - 9.81;
	//init speed is 1.0m/s
	state_matrix_ << theta1_ * theta4_ / 1.0, theta2_ * theta4_ / 1.0 - 1.0, -h_ * d_phi_ / ix_, theta5_, 0, 0,
		theta2_ / iz_ / 1.0, theta3_ / iz_ / 1.0, 0, 0, 0, 0,
		h_ * theta1_ / ix_ / 1.0, h_ * theta2_ / ix_ / 1.0, -d_phi_ / ix_, -k_phi_ / ix_, 0, 0,
		0, 0, 1, 0, 0, 0,
		1, 0, 0, 0, 0, 1.0,
		0, 1, 0, 0, 0, 0;
	base_state_matrix_ = state_matrix_;
	control_matrix_ << -cf_ * theta4_,
		-lf_ * cf_ / iz_,
		-h_ * cf_ / ix_,
		0,
		0,
		0;
	distur_matrix_ << h_ * k_phi_ / ix_, 0,
		0, 0,
		k_phi_ / ix_, 0,
		0, 0,
		0, 0,
		0, -1.0;
	out_state_matrix_ << 0, 0, 0, 0, 1.0, 0;
	out_distur_matrix_ << 0, 0;
}
void VehicleLinearLv::speed_update_matrix(double speed)
{
	if (std::abs(speed) < speed_threshold_)
	{
		if (speed >= 0)
			speed = speed_threshold_;
		else
			speed = -speed_threshold_;
	}

	state_matrix_(0, 0) = base_state_matrix_(0, 0) / speed;
	state_matrix_(0, 1) = theta2_ * theta4_ / speed - speed;
	state_matrix_(1, 0) = base_state_matrix_(1, 0) / speed;
	state_matrix_(1, 1) = base_state_matrix_(1, 1) / speed;
	state_matrix_(2, 0) = base_state_matrix_(2, 0) / speed;
	state_matrix_(2, 1) = base_state_matrix_(2, 1) / speed;

	state_matrix_(4, 5) = speed;
	std::cout << "$$$$$$$$$$$$$$$$$$$" << std::endl;
	std::cout << distur_matrix_.rows() << std::endl;
	std::cout << distur_matrix_.cols() << std::endl;
	distur_matrix_(5, 1) = -speed;
}

double VehicleLinearLv::mass()
{
	return mass_;
}
double VehicleLinearLv::lf()
{
	return lf_;
}
double VehicleLinearLv::lr()
{
	return lr_;
}
double VehicleLinearLv::cf()
{
	return cf_;
}
double VehicleLinearLv::cr()
{
	return cr_;
}
double VehicleLinearLv::iz()
{
	return iz_;
}
double VehicleLinearLv::max_tire_f_angle()
{
	return max_tire_f_angle_;
}
double VehicleLinearLv::speed_threshold()
{
	return speed_threshold_;
}
Eigen::MatrixXd VehicleLinearLv::base_state_matrix()
{
	return base_state_matrix_;
}

} // namespace control
} // namespace auto_ros